#include "ismfortest/ismtest.hpp" 
#include <iostream> 
#include <memory>

namespace ismtest
{
    ismtester::ismtester() : Node("ismtester")
    {
        // 基本状态初始化
        is_return_operation_ = false;
        current_state_ = JsonData_convert::State::STATE_IDLE;
        state_timer_ = 0;

        // ui控制变量
        command_action_ = "";
        target_well_id_ = 0;

        // 系统状态变量
        robot_id_ = "fangshe";
        battery_charging_ = false;
        run_time_ = 0.0;

        // 机器人位姿变量
        robot_pose_.x = 3.5;
        robot_pose_.y = 2.4;
        robot_pose_.theta = 0.0;
        frame_id_ = "map";

        // 机器人任务状态
        task_stage_ = "idle";

        // 抓手状态
        gripper_loaded_ = JsonData_convert::GripperState::NONE;

        // 放射井状态
        current_well_id_ = 0;
        current_well_has_source_ = true;
        current_well_has_cover_ = true;

        // 创建指令订阅者
        command_subscriber_ = this->create_subscription<std_msgs::msg::String>("/fangshe/robot/control/command", 10,
                                std::bind(&ismtester::commandCallback, this, std::placeholders::_1));
        // 创建急停订阅者
        stop_subscriber_ = this->create_subscription<std_msgs::msg::String>("/fangshe/robot/control/stop", 10,
                                std::bind(&ismtester::emergencystopCallback, this, std::placeholders::_1));

        // 指令接收状态发布者
        // command_received_publisher_= this->create_publisher<std_msgs::msg::String>("/fangshe/robot/status/received", 10);
        
        // 创建系统状态发布者
        system_status_publisher_ = this->create_publisher<std_msgs::msg::String>("/fangshe/robot/status/system", 10);
        pose_publisher_ = this->create_publisher<std_msgs::msg::String>("/fangshe/robot/status/pose", 10);

        // 创建任务状态发布者
        task_status_publisher_ = this->create_publisher<std_msgs::msg::String>("/fangshe/robot/status/task", 10);
        gripper_status_publisher_ = this->create_publisher<std_msgs::msg::String>("/fangshe/robot/status/gripper", 10);
        well_status_publisher_ = this->create_publisher<std_msgs::msg::String>("/fangshe/robot/status/well", 10);

        // 创建定时器
        timer_ = this->create_wall_timer(1000ms, std::bind(&ismtester::timerCallback, this));

        RCLCPP_INFO(this->get_logger(), "放射源状态机已启动，正在等待指令发布...");

        // 发布初始状态
        publishSystemStatus();
        publishPoseStatus();
        publishTaskStatus();
        publishGripperStatus();
        publishWellStatus();
    }

    void ismtester::commandCallback(const std_msgs::msg::String::SharedPtr msg)
    {
        // 解析由mqtt转发过来的指令
        try
        {
            JsonData_convert command;

            if(command.parse_from_json(msg->data))
            {
                target_well_id_ = command.get_well_id();
                command_action_ = command.get_action();

                RCLCPP_INFO(this->get_logger(), "收到%s指令,准备前往%d号放射坑!", 
                   command_action_.c_str(), target_well_id_);
                
                // 根据指令设置操作类型
                if(command_action_ == "extract") {
                    is_return_operation_ = false;
                    current_state_ = JsonData_convert::State::STATE_ENTERING_WEARHOUSE;
                    task_stage_ = "entering_warehouse";
                } else if(command_action_ == "return") {
                    is_return_operation_ = true;
                    current_state_ = JsonData_convert::State::STATE_RECEIVING_COMMAND;
                    task_stage_ = "receiving_command";
                } else {
                    RCLCPP_WARN(this->get_logger(), "未知的指令类型: %s", command_action_.c_str());
                    return;
                }

                // 发布确认消息
                publishSystemStatus();
                publishTaskStatus();
                
                RCLCPP_INFO(this->get_logger(), "%s", command.get_formatted_message().c_str());
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "JSON解析失败");
            }
        }
        catch(const std::exception& e)
        {
            RCLCPP_ERROR(this->get_logger(), "解析指令时出错: %s", e.what());
        }
    }

    void ismtester::emergencystopCallback(const std_msgs::msg::String::SharedPtr msg)
    {

    }


    
    std::string ismtester::getCurrentTimestamp()
    {
        auto now = std::chrono::system_clock::now();
        auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
        auto time_t = std::chrono::system_clock::to_time_t(now);
        
        std::ostringstream oss;
        oss << std::put_time(std::localtime(&time_t), "%Y-%m-%d %H:%M:%S");
        oss << '.' << std::setfill('0') << std::setw(3) << ms.count();
        return oss.str();
    }

    // 系统状态发布函数
    void ismtester::publishSystemStatus()
    {
        auto msg = std::make_unique<std_msgs::msg::String>();

        // 使用JsonData_convert设置并生成系统状态JSON
        json_converter_.set_system_status(robot_id_, battery_charging_, run_time_);
        msg->data = json_converter_.get_system_status_json();

        system_status_publisher_->publish(std::move(msg));
        RCLCPP_DEBUG(this->get_logger(), "发布系统状态: %s", msg->data.c_str());
    }

    // 位姿状态发布函数
    void ismtester::publishPoseStatus()
    {
        auto msg = std::make_unique<std_msgs::msg::String>();

        json_converter_.set_pose_status(robot_pose_.x, robot_pose_.y, robot_pose_.theta, frame_id_);
        msg->data = json_converter_.get_pose_status_json();
        
        pose_publisher_->publish(std::move(msg));
        RCLCPP_DEBUG(this->get_logger(), "发布位姿状态: %s", msg->data.c_str());
    }

    // 当前执行任务发布函数
    void ismtester::publishTaskStatus()
    {
        auto msg = std::make_unique<std_msgs::msg::String>();
        
        // 使用JsonData_convert根据当前状态生成任务状态JSON
        msg->data = json_converter_.get_task_status_json(current_state_);
        
        task_status_publisher_->publish(std::move(msg));
        RCLCPP_DEBUG(this->get_logger(), "发布任务状态: %s", msg->data.c_str());
    }

    // 抓手状态发布函数
    void ismtester::publishGripperStatus()
    {
        auto msg = std::make_unique<std_msgs::msg::String>();
        
        // 使用JsonData_convert生成抓手状态JSON
        msg->data = json_converter_.get_gripper_status_json(gripper_loaded_);
        
        gripper_status_publisher_->publish(std::move(msg));
        RCLCPP_DEBUG(this->get_logger(), "发布抓手状态: %s", msg->data.c_str());
    }

    // 井状态发布函数
    void ismtester::publishWellStatus()
    {
        auto msg = std::make_unique<std_msgs::msg::String>();
        
        // 使用JsonData_convert生成井状态JSON
        msg->data = json_converter_.get_well_status_json(current_well_id_, current_well_has_source_, current_well_has_cover_);
        
        well_status_publisher_->publish(std::move(msg));
        RCLCPP_DEBUG(this->get_logger(), "发布井状态: %s", msg->data.c_str());
    }

    void ismtester::processStateMachine()
    {
        switch(current_state_) {
            case JsonData_convert::State::STATE_IDLE:
                // 空闲状态，等待指令
                task_stage_ = "idle";
                break;
                
            case JsonData_convert::State::STATE_RECEIVING_COMMAND:
                // 接收到指令，开始处理
                task_stage_ = "receiving_command";
                if(is_return_operation_) {
                    current_state_ = JsonData_convert::State::STATE_NAVIGATING_TO_WELL;
                    task_stage_ = "navigating_to_well";
                }
                break;
                
            case JsonData_convert::State::STATE_ENTERING_WEARHOUSE:
                // 模拟进入仓库过程
                state_timer_++;
                if(state_timer_ >= 3) { // 模拟3秒后到达
                    current_state_ = JsonData_convert::State::STATE_NAVIGATING_TO_WELL;
                    state_timer_ = 0;
                    task_stage_ = "navigating_to_well";
                }
                break;
                
            case JsonData_convert::State::STATE_NAVIGATING_TO_WELL:
                // 模拟导航过程
                state_timer_++;
                if(state_timer_ >= 5) { // 模拟5秒后到达
                    current_state_ = JsonData_convert::State::STATE_ARRIVED_AT_WELL;
                    state_timer_ = 0;
                    task_stage_ = "arrived_at_well";
                }
                break;
                
            case JsonData_convert::State::STATE_ARRIVED_AT_WELL:
                current_state_ = JsonData_convert::State::STATE_REMOVING_WELL;
                task_stage_ = "removing_well";
                break;
                
            case JsonData_convert::State::STATE_REMOVING_WELL:
                state_timer_++;
                if(state_timer_ >= 2) {
                    current_state_ = JsonData_convert::State::STATE_WELL_REMOVED;
                    state_timer_ = 0;
                    current_well_has_cover_ = false;
                    gripper_loaded_ = JsonData_convert::GripperState::COVER;
                    task_stage_ = "well_removed";
                }
                break;
                
            case JsonData_convert::State::STATE_WELL_REMOVED:
                if(!is_return_operation_) {
                    current_state_ = JsonData_convert::State::STATE_EXTRACTING_SOURCE;
                    task_stage_ = "extracting_source";
                } else {
                    current_state_ = JsonData_convert::State::STATE_DELIVERING_SOURCE;
                    task_stage_ = "delivering_source";
                }
                break;
                
            case JsonData_convert::State::STATE_EXTRACTING_SOURCE:
                state_timer_++;
                if(state_timer_ >= 2) {
                    current_state_ = JsonData_convert::State::STATE_SOURCE_EXTRACTED;
                    state_timer_ = 0;
                    current_well_has_source_ = false;
                    gripper_loaded_ = JsonData_convert::GripperState::SOURCE;
                    task_stage_ = "source_extracted";
                }
                break;
                
            case JsonData_convert::State::STATE_SOURCE_EXTRACTED:
                if(is_return_operation_) {
                    current_state_ = JsonData_convert::State::STATE_REPLACING_WELL;
                    task_stage_ = "replacing_well";
                } else {
                    current_state_ = JsonData_convert::State::STATE_DELIVERING_SOURCE;
                    task_stage_ = "delivering_source";
                }
                break;
                
            case JsonData_convert::State::STATE_DELIVERING_SOURCE:
                state_timer_++;
                if(state_timer_ >= 2) {
                    current_state_ = JsonData_convert::State::STATE_SOURCE_DELIVERED;
                    state_timer_ = 0;
                    current_well_has_source_ = true;
                    gripper_loaded_ = JsonData_convert::GripperState::NONE;
                    task_stage_ = "source_delivered";
                }
                break;
                
            case JsonData_convert::State::STATE_SOURCE_DELIVERED:
                current_state_ = JsonData_convert::State::STATE_REPLACING_WELL;
                task_stage_ = "replacing_well";
                break;
                
            case JsonData_convert::State::STATE_REPLACING_WELL:
                state_timer_++;
                if(state_timer_ >= 2) {
                    current_state_ = JsonData_convert::State::STATE_WELL_REPLACED;
                    state_timer_ = 0;
                    current_well_has_cover_ = true;
                    gripper_loaded_ = JsonData_convert::GripperState::NONE;
                    task_stage_ = "well_replaced";
                }
                break;
                
            case JsonData_convert::State::STATE_WELL_REPLACED:
                current_state_ = JsonData_convert::State::STATE_IDLE;
                task_stage_ = "idle";
                break;
                
            default:
                break;
        }
    }

    void ismtester::timerCallback()
    {
        // 更新运行时间
        run_time_ += 1.0;
        
        // 处理状态机
        processStateMachine();
        
        // 发布周期性状态
        publishSystemStatus();
        publishPoseStatus();
        publishTaskStatus();
        publishGripperStatus();
        publishWellStatus();
    }

} // namespace ismtest

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ismtest::ismtester>());
    rclcpp::shutdown();
    return 0;
}